using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;

namespace Dida
{
	/// <summary>
	/// This class contains the settings that are used while building
	/// a <see cref = "CollisionTree">CollisionTree</see> from a list
	/// of triangles. (using the
	/// <see cref = "CollisionTree(CollisionTriangle[],CollisionTreeSettings)"/>constructor).
	/// </summary>
	public class CollisionTreeSettings
	{
		int maxTrisPerNode = 16;
		float maxOverlap = .3f;

		/// <summary>
		/// Creates a new CollisionTreeSettings object, that contains
		/// the default settings.
		/// </summary>
		public CollisionTreeSettings() { }

		/// <summary>
		/// Gets or sets the maximum number of triangles a node can contain.
		/// If a node exceeds this limit, it will be split into child nodes.
		/// </summary>
		public int MaxTrisPerNode
		{
			get { return maxTrisPerNode; }
			set { maxTrisPerNode = value; }
		}

		public float MaxOverlap
		{
			get { return maxOverlap; }
			set { maxOverlap = value; }
		}
	}

	public class CollisionTree
	{
		Node root;

		Sphere fittingBoundingSphere;
		Sphere pivottedBoundingSphere;

		/// <summary>
		/// Builds a new CollisionTree, using the given triangles
		/// </summary>
		/// <param name="triangles">The triangles to build a CollisionTree for</param>
		/// <param name="settings">The settings used to create this tree. 
		/// See <see cref = "CollisionTreeSettings">CollisionTreeSettings</see>
		/// for a description of the settings.
		/// </param>
		public CollisionTree(CollisionTriangle[] triangles,CollisionTreeSettings settings)
		{
			List<CollisionTriangle> trianglesList = new List<CollisionTriangle>(triangles);
			root = new Node(ref trianglesList, settings);

			CalcBoundingVolumes();
		}

		public CollisionTree(MeshLoader meshLoader, CollisionTreeSettings settings)
		{
			List<CollisionTriangle> tris = new List<CollisionTriangle>();
			tris.Capacity = meshLoader.Triangles.Count;
			foreach (MeshLoader.Triangle srcTri in meshLoader.Triangles)
			{
				CollisionTriangle tri = new CollisionTriangle(
					srcTri.Vertices[0].pos,
					srcTri.Vertices[1].pos,
					srcTri.Vertices[2].pos);
				tris.Add(tri);
			}

			root = new Node(ref tris, settings);

			CalcBoundingVolumes();
		}

		private void CalcBoundingVolumes()
		{
			fittingBoundingSphere = new Sphere(BoundingBox.Center, 0);
			root.GrowBoundingSphere(ref fittingBoundingSphere);
			fittingBoundingSphere.radius = (float)Math.Sqrt(fittingBoundingSphere.radius);

			pivottedBoundingSphere = new Sphere(0, 0, 0, 0);
			root.GrowBoundingSphere(ref pivottedBoundingSphere);
			pivottedBoundingSphere.radius = (float)Math.Sqrt(pivottedBoundingSphere.radius);
		}

		/// <summary>
		/// <para>
		/// Tests whether the sphere collides with any of the triangles
		/// in this CollisionTree, on the path specified by dir. If there
		/// was a collision, it will return true, and res will be filled
		/// in with the information about the collision. If no collision 
		/// occured, it will return false.
		/// </para>
		/// <para>
		/// This collision test uses a sweep test, which means that it will return
		/// the exact position on the path where the collision occured.
		/// Even if the sphere doesn't intersect the triangle on it's start 
		/// position nor it's end position, but if it does intersect it 
		/// somewhere inbetween, the collision will be detected. 
		/// </para>
		/// <para>
		/// It is also designed to be very stable. It doesn't matter how
		/// deep you are already inside the triangle, if you try to go deeper,
		/// it will be detected as a collision, and if you try to move out, 
		/// no collision will be detected. This makes it almost impossible to
		/// get trough the collision world, or to get stuck inside it.
		/// </para>
		/// </summary>
		/// <param name="sphere">The sphere you want to do the collision test for</param>
		/// <param name="dir">The direction the sphere is moving</param>
		/// <param name="res">The CollisionResult that will be filled in
		/// with the result of the collision, if one occurs. If no collision
		/// occured, it will be null.<see ref = "CollisionResult">
		/// CollisionResult</see></param>
		/// <returns>returns whether there's a collision at all</returns>
		public bool TestForCollision(Sphere sphere, Vector3D dir,
			out CollisionResult res)
		{
			Sphere pathBSphere = new Sphere(sphere.center + dir * .5f,
				sphere.radius + dir.Len * .5f);
			return root.TestForCollision(sphere, dir, out res, pathBSphere);
		}

		public void WriteBinary(BinaryWriter writer)
		{
			root.WriteBinary(writer);
			fittingBoundingSphere.WriteBinary(writer);
			pivottedBoundingSphere.WriteBinary(writer);
		}

		public void DrawNodeBoundingBoxes(IGeometryDebugDraw debugDraw,UInt32 colour)
		{
			root.DrawNodeBoundingBoxes(debugDraw,colour);
		}

		public void DrawToDebugMesh(IGeometryDebugMesh mesh)
		{
			root.DrawToDebugMesh(mesh);
		}

		private class Node
		{
			Box boundingBox;
			
			Node lowNode;
			Node highNode;

			CollisionTriangle[] triangles;

			public Node(ref List<CollisionTriangle> triangles,CollisionTreeSettings settings)
			{
				// calc the bounding box
				boundingBox = Box.NullBox;
				foreach(CollisionTriangle tri in triangles)
					boundingBox.Union(tri.BoundingBox);

				if (triangles.Count > settings.MaxTrisPerNode)
				{
					List<CollisionTriangle> thisNodeTris;
					List<CollisionTriangle> lowTris;
					List<CollisionTriangle> highTris;
					DivideTriangles(triangles, out thisNodeTris, out lowTris,
						out highTris, settings);

					// yup, i'm from a c++ background, and I'm always afraid of
					// memory allocations.
					triangles = null;

					if (thisNodeTris != null)
						this.triangles = thisNodeTris.ToArray();

					if (lowTris != null)
						lowNode = new Node(ref lowTris, settings);
					if (highTris != null)
						highNode = new Node(ref highTris, settings);					
				}
				else
				{
					this.triangles = triangles.ToArray();
				}
			}

			private void DivideTriangles(List<CollisionTriangle> triangles,
				out List<CollisionTriangle> thisNodeTriangles,
				out List<CollisionTriangle> lowTris,
				out List<CollisionTriangle> highTris,
				CollisionTreeSettings settings)
			{
				int splitAxis;

				// find the largest axis
				Vector3D size = boundingBox.max - boundingBox.min;
				if (size.x > size.y && size.x > size.z)
					splitAxis = 0;
				else if (size.y > size.z)
					splitAxis = 1;
				else
					splitAxis = 2;
				
				triangles.Sort(new TriangleCenterComparer(splitAxis).Compare);

				int mid = triangles.Count / 2;
				lowTris = new List<CollisionTriangle>(mid);
				highTris = new List<CollisionTriangle>(triangles.Count - mid);
				thisNodeTriangles = new List<CollisionTriangle>();

				Box lowBBox = Box.NullBox;
				for (int i = 0; i < mid; i++)
				{
					lowTris.Add(triangles[i]);
					lowBBox.Union(triangles[i].BoundingBox);
				}

				Box highBBox = Box.NullBox;
				for (int i = mid; i < triangles.Count; i++)
				{
					highTris.Add(triangles[i]);
					highBBox.Union(triangles[i].BoundingBox);
				}

				float midPos = (lowBBox.max[splitAxis] + highBBox.min[splitAxis]) * .5f;
				float maxOverlapPos = midPos + (highBBox.max[splitAxis] -
					midPos) * settings.MaxOverlap;
				for (int i = 0; i < lowTris.Count;)
				{
					if (lowTris[i].BoundingBox.max[splitAxis] > maxOverlapPos)
					{
						thisNodeTriangles.Add(lowTris[i]);
						lowTris.RemoveAt(i);
					}
					else
						i++;
				}
				if (lowTris.Count == 0)
					lowTris = null;

				maxOverlapPos = midPos + (lowBBox.min[splitAxis] -
					midPos) * settings.MaxOverlap;
				for (int i = 0; i < lowTris.Count; )
				{
					if (highTris[i].BoundingBox.max[splitAxis] < maxOverlapPos)
					{
						thisNodeTriangles.Add(highTris[i]);
						highTris.RemoveAt(i);
					}
					else
						i++;
				}
				if (highTris.Count == 0)
					highTris = null;
			}

			public bool TestForCollision(Sphere sphere, Vector3D dir,
				out CollisionResult res, Sphere pathBSphere)
			{
				res = null;
				if(!sphere.Intersects(pathBSphere))
					return false;
				
				foreach (CollisionTriangle tri in triangles)
				{
					CollisionResult curRes;
					if (tri.TestForCollision(sphere, dir, out curRes, pathBSphere))
					{
						if (res == null)
							res = curRes;
						else if (curRes.dist < res.dist)
							res = curRes;
					}
				}

				if (lowNode != null)
				{
					CollisionResult lowNodeRes;
					if (lowNode.TestForCollision(sphere, dir,
						out lowNodeRes, pathBSphere))
					{
						if (res == null)
							res = lowNodeRes;
						else if (lowNodeRes.dist < res.dist)
							res = lowNodeRes;
					}
				}

				if (highNode != null)
				{
					CollisionResult highNodeRes;
					if (highNode.TestForCollision(sphere, dir,
						out highNodeRes, pathBSphere))
					{
						if (res == null)
							res = highNodeRes;
						else if (highNodeRes.dist < res.dist)
							res = highNodeRes;
					}
				}

				return res != null;
			}

			// like TestForCollision, but only tests the triangles, not
			// the child nodes
			public bool TestTrianglesForCollision(Sphere sphere, Vector3D dir,
				out CollisionResult res, Sphere pathBSphere)
			{
				res = null;
				if (!sphere.Intersects(pathBSphere))
					return false;

				foreach (CollisionTriangle tri in triangles)
				{
					CollisionResult curRes;
					if (tri.TestForCollision(sphere, dir, out curRes, pathBSphere))
					{
						if (res == null)
							res = curRes;
						else if (curRes.dist < res.dist)
							res = curRes;
					}
				}

				return res != null;
			}

			public void WriteBinary(BinaryWriter writer)
			{
				boundingBox.WriteBinary(writer);

				writer.Write(triangles.Length);
				foreach (CollisionTriangle tri in triangles)
					tri.WriteBinary(writer);

				if (lowNode != null)
				{
					writer.Write(true);
					lowNode.WriteBinary(writer);
					highNode.WriteBinary(writer);
				}
				else
					writer.Write(false);
			}

			public Box BoundingBox
			{
				get { return boundingBox; }
			}

			/// <summary>
			/// <para>
			/// Grows the given sphere, so that it fits this node aswell.
			/// The center of the sphere won't be changed
			/// </para>
			/// <para>
			/// NOTE: this method assumes the radius of the sphere is in fact
			/// the squared radius.
			/// </para>
			/// </summary>
			/// <param name="sphere">The sphere to grow.</param>
			public void GrowBoundingSphere(ref Sphere sphere)
			{
				foreach (CollisionTriangle tri in triangles)
				{
					float rad1 = (tri.GetVertex(0) - sphere.center).SqrLen;
					if (rad1 > sphere.radius)
						sphere.radius = rad1;

					float rad2 = (tri.GetVertex(1) - sphere.center).SqrLen;
					if(rad2 > sphere.radius)
						sphere.radius = rad2;

					float rad3 = (tri.GetVertex(2) - sphere.center).SqrLen;
					if (rad3 > sphere.radius)
						sphere.radius = rad3;
				}

				if (lowNode != null)
				{
					lowNode.GrowBoundingSphere(ref sphere);
					highNode.GrowBoundingSphere(ref sphere);
				}
			}

			public void GetNodesInSphere(Sphere sphere, NodeList nodeList)
			{
				if (boundingBox.Intersects(sphere))
				{
					nodeList.AddNode(this);

					if (lowNode != null)
						lowNode.GetNodesInSphere(sphere, nodeList);
					if (highNode != null)
						highNode.GetNodesInSphere(sphere, nodeList);
				}
			}

			public void DrawNodeBoundingBoxes(IGeometryDebugDraw debugDraw, UInt32 colour)
			{
				debugDraw.DrawBox(boundingBox, colour);
				if (highNode != null)
					highNode.DrawNodeBoundingBoxes(debugDraw, colour);
				if (lowNode != null)
					lowNode.DrawNodeBoundingBoxes(debugDraw, colour);
			}

			public void DrawToDebugMesh(IGeometryDebugMesh mesh)
			{
				foreach (CollisionTriangle tri in triangles)
					mesh.AddTriangle(tri.GetVertex(0), tri.GetVertex(1), tri.GetVertex(2));
				if (lowNode != null)
					lowNode.DrawToDebugMesh(mesh);
				if (highNode != null)
					highNode.DrawToDebugMesh(mesh);
			}
		}

		class TriangleCenterComparer : IComparer
		{
			private int i;

			public TriangleCenterComparer(int axis)
			{
				i = axis;
			}

			public int Compare(object x, object y)
			{
				CollisionTriangle tri1 = (CollisionTriangle)x;
				CollisionTriangle tri2 = (CollisionTriangle)y;

				if (tri1.BoundingBox.Center[i] < tri2.BoundingBox.Center[i])
					return -1;
				else if (tri1.BoundingBox.Center[i] == tri2.BoundingBox.Center[i])
					return 0;
				else
					return 1;
			}
		}

		public class NodeList
		{
			private CollisionTree tree;
			private List<Node> nodes = new List<Node>();

			internal NodeList(CollisionTree tree)
			{
				this.tree = tree;
			}

			internal void AddNode(object node)
			{
				nodes.Add((Node)node);
			}

			public bool TestForCollision(Sphere sphere, Vector3D dir,
				out CollisionResult res)
			{
				res = null;

				Sphere pathBSphere = sphere.Clone();
				pathBSphere.center += dir * .5f;
				pathBSphere.radius += dir.Len * .5f;

				foreach (Node node in nodes)
				{
					CollisionResult curRes;
					if (node.TestTrianglesForCollision(sphere, dir, 
						out curRes, pathBSphere))
					{
						if (res == null)
							res = curRes;
						else if (curRes.dist < res.dist)
							res = curRes;
					}
				}

				return res != null;
			}
		}

		/// <summary>
		/// <para>
		/// Creates a list of the nodes that intersect the given sphere.
		/// </para>
		/// <para>
		/// This can be used when you know you are going to do many collision
		/// tests on the same area. Then you can use this method to find
		/// the nodes that are in the area, and then do your collision tests
		/// using this <see cref="NodeList">NodeList</see>. Doing it this way
		/// has the advantage that the tree doesn't need to be walked through
		/// each time, and will thus result in a speedup.
		/// </para>
		/// </summary>
		/// <param name="sphere">The sphere that specifies the
		/// area of which you want to find the nodes. This sphere 
		/// should use the local coordinates relative to this CollisionTree.</param>
		/// <returns>The <see cref = "NodeList">NodeList</see></returns>
		public NodeList GetNodesInSphere(Sphere sphere)
		{
			NodeList ret = new NodeList(this);
			root.GetNodesInSphere(sphere, ret);
			return ret;
		}

		/// <summary>
		/// Gets the bounding box of the geometry in this tree.
		/// </summary>
		public Box BoundingBox
		{
			get { return root.BoundingBox; }
		}

		/// <summary>
		/// Gets a bounding sphere that tightly fits all the geometry of this tree.
		/// </summary>
		/// <seealso cref="PivottedBoundingSphere"/>
		public Sphere FittingBoundingSphere
		{
			get { return fittingBoundingSphere; }
		}

		/// <summary>
		/// Gets a bounding sphere, that has it's center on the pivot of 
		/// this tree (0,0,0 in local space), and encompasses all the content
		/// of the tree.
		/// </summary>
		/// <seealso cref="FittingBoundingSphere"/>
		public Sphere PivottedBoundingSphere
		{
			get { return pivottedBoundingSphere; }
		}
	}
}